clear
% the order of breakpoints listed below is the order used in table lookup
% data.
MS=.01;% Max sample interval
global RG
MAX_PULSES=1;% max number of allowed pulses.  Only allow one pulse per iteration until LUTS are populated, otherwise we'll get noise out of the LUTS
RG=1000;% rounding gain
IFA=round(0*RG)/RG; % initial foot angle
IHA=round(.1*RG)/RG;% initial hip angle
%Note: initial values of breakpoint vectors should be greater than the absolute limits
%of the system to prevent out-of-bound values
%LUT dim. 1
APFBP=[-2.5*pi IFA 2.5*pi];% angular position of foot breakpoint vector
%LUT dim. 2
AVFBP=[-2 0 2];% velocity in radians per second.
%LUT dim. 3
AAFBP=[-2 0 2];
%LUT dim 4
APHBP=[-2.5*pi IHA 2.5*pi];% angular position of hip breakpoint vector
TLDT=zeros(3,3,3,3); % Table lookup data for acceleration pulse 1/2period.  Note that there are as many dimensions as breakpoint vectors, each dimension has a size = corresponding breakpoint vector length
TLDAF=zeros(3,3,3,3);
TLDST1=zeros(3,3,3,3);
%Find initial acceleration due to initia position and velocity.  LIke
%"hefting" to guage object parameters
APFBP=floor(APFBP*RG)/RG;
AVFBP=floor(AVFBP*RG)/RG;
AAFBP=floor(AAFBP*RG)/RG;
APHBP=floor(APHBP*RG)/RG;
[t,x,apf,avf,aaf,aph,apf_pe,avf_pe,aaf_pe,aph_pe,clk_pe,pulse_cnt]=sim('ipend3',.5);
DATA=[apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end),clk_pe(end)];
%LUT dim. 3
APForder=2;% initial ap data is IFA
AVForder=2;% initial av is 0
%Run simulation once to find acceleration due to initial conditions
if aaf(1)~=0 % break point vector must be monotonic, so don't duplicate 0 value
%     [AAFBP AAForder]=sort([init_aaf_val AAFBP]);% first data point in the acceleration break point array.
%     AAForder=find(AAFBP==init_aaf_val);% find where new value is inserted into breakpoint array
    [TLDT,TLDAF,TLDST1,AAFBP,AAForder]=InsertBlankRow(TLDT,TLDAF,TLDST1,AAFBP,aaf_pe(end),3);
    
else
    AAForder=2;% initial aa data is 0
end
APHorder=2;% initial aph data is 0
AF=.5*1/apf_pe(end);% initial high energy "kick".  determined experimentalyy to cause convergence
T=round(.1*RG)/RG;% initalize T to about .1 seconds.  Reduce Pulse width until final APH=AF*100 for input
ST1=round(0*RG)/RG;
sim_time=2;% length of time to run simulation
do_adj=0;% adjust parameters of an acceleration pulse until desired behavior is achieved
%% BEGIN RIGHTING BEHAVIOR
%emit initial guess.  parameters determined experimentally to cause foot
%angle to cross vertical with enogh velocity to continue a monotonic increase
%in foot angle, velocity should be relatively high, APH should be around .4
%radians.

% there is a desired shape to aph.  Each succcessive pulse should reduce
% |aph| by max_aph/4 until max aph<=value that causes top of pendulum to
% move <=1cm.  (.05, for instance)

% T should be adjusted so that max aph=AF/100
%%
TLDT(APForder,AVForder,AAForder,APHorder)=T;
TLDAF(APForder,AVForder,AAForder,APHorder)=136.704;%AF;
TLDST1(APForder,AVForder,AAForder,APHorder)=ST1;
MAX_PULSES=MAX_PULSES+1;% allow one additional pulse for next trial
[t,x,apf,avf,aaf,aph,apf_pe,avf_pe,aaf_pe,aph_pe,clk_pe,pulse_cnt]=sim('ipend3',sim_time+3);
%%
prev_pulse_aaf_sign=0;% direction of foot angular acceleration due to the previous pulse. -1=clockwise
last_AF=AF;% remember the last applied force 
target_aph=-.01;% adjust AF so APH reaches this magnitude
while 1
    %change parameters of motion
    sim_time=sim_time+1.5;
    cur_pulse_aaf_sign=0;
    good_sign=-1;
    good_min_val=-1;% test of value to minimize is changing in the desired direction
    prev_good_min_val=-1;% valid values are 0/1, -1 indicates unintialized
    prev_good_sign=-1;
    loop_counter=0;% count time through loop  
    goal_reached=0;
    param_arr=[100*(target_aph-aph_pe(end)),.1,0];% AF,T,ST1 values    
    param_arr_idx_vec=[1 3 -1];% sequence of array indices to alter.  Terminate process when -1 is reached
    param_arr_idx_vec_idx=1;
    param_arr_idx=param_arr_idx_vec(param_arr_idx_vec_idx);% which parameter are we currently adjusting?=
    par_adj_level=[(10*250)/RG,.25];% parameter adjustment level, begin adjusting applied force at same magnitude as previous pulse, adjust time at minimum possible
    par_adj_dir=[-1*sign(last_AF),1];% parameter adjustment direction can take values 1(positive) -1 ( nehative)
    cur_pulse_count=pulse_cnt(end);% be sure only one pusle is generated, for now.Multiple pulses means system is trying to interpolate values in the LUTs, which won't be valid until training is complete
    prev_pulse_cnt=cur_pulse_count;% there is one pulse at time=0
    cur_min_val=100000;%initialize to some value that can never be attained by the system
    prev_min_val=100000;
    prev_value=[];% history of parameter values used during search   
%     while do_adj
%         loop_counter=loop_counter+1;
        %emit new guess        
%         TLDAF(APForder,AVForder,AAForder,APHorder)=param_arr(1);
%         TLDT(APForder,AVForder,AAForder,APHorder)=T;% for now, T is constant     
%         TLDST1(APForder,AVForder,AAForder,APHorder)=param_arr(3);
%         [t,x,apf,avf,aaf,aph,apf_pe,avf_pe,aaf_pe,aph_pe,clk_pe,pulse_cnt]=sim('ipend3',sim_time);    
%         %measure performace
%         idx=min(find(clk_pe==clk_pe(end)));% the vector element index of the end-of-pulse time
%         % find the time of the avf zero crossing closest to the pulse end.
%         t_idx_avf0_x=crossing(avf);
%         t_idx_avf0_x=t_idx_avf0_x(min(find(t_idx_avf0_x>=idx)));% find the avf zero crossing time index greater than or equal to the pulse end time                
%         if isempty(t_idx_avf0_x)||any(abs(apf(1:t_idx_avf0_x))>=(pi/2))% if avf never crosses 0 or it crosses 0 after apf crosses pi/2
%             cur_pulse_aaf_sign=sign(avf(idx));% the reliable indicator of long term trend in acceleration changes depending on wether apf crosses 0 or not
%         else
%             cur_pulse_aaf_sign=sign(aaf(t_idx_avf0_x));% direction of foot angular acceleration due to the previous pulse. -1=clockwise
%         end
%         cur_pulse_count=pulse_cnt(end);
%         if loop_counter>=2
%             prev_min_val=cur_min_val;% store previous apf value
%             prev_good_sign=good_sign;
%             prev_good_min_val=good_min_val;
%         end    
%         %if there is no change in sign or apf or more than one acceleration pulse is generated, continue adjusting parameter
%         %at current rate. Note, system may begin in undesired state and
%         %only enter desired state after a certain parameter value is
%         %achieved
%         
%         if ((cur_pulse_count==(1+prev_pulse_cnt))||(cur_pulse_count==prev_pulse_cnt))&&(loop_counter>=2)% check that only one new pulse is produced
%             good_sign=(cur_pulse_aaf_sign==-prev_pulse_aaf_sign);% do curretn parameters cause motion in the opposite direction to previous pulse?
%             prev_pulse_cnt=cur_pulse_count;
%             if good_sign==1% then minimize aaf that occurs at end of acceleration pulse
%                 cur_min_val=aaf(idx);
%             else% minimize the velocity that occurs at end of acceleration pulse
%                 cur_min_val=avf(idx);
%             end% if good_sign==1
%                     
%             if ((prev_good_sign==0) && (good_sign==1)) ||((prev_good_sign==1) && (good_sign==0))
%                 par_adj_level(param_arr_idx_vec_idx)=par_adj_level(param_arr_idx_vec_idx)/2;
%                 par_adj_dir(param_arr_idx_vec_idx)=-par_adj_dir(param_arr_idx_vec_idx);                 
%             end
% %             if (cur_min_val~=prev_min_val)% continue with current parameter adjustment scheme until a change occurs.  Since small changes may not cause mesurable output, this check is necesary
% %                 if ((prev_good_sign==0) && (good_sign==1)) ||((prev_good_sign==1) && (good_sign==0))% then direction of system acceleration has switched. reset the minimization routine
% %                     good_min_val=1;% set initial velocity to good
% %                     prev_good_min_val=0;
% %                 elseif ((prev_good_sign==1) && (good_sign==1))||((prev_good_sign==0) && (good_sign==0))% then last trial and current trial both produce the same direction of system state change
% %                     good_min_val=abs(cur_min_val)<abs(prev_min_val);% do current parameters cause system to move close to equilibrium for longer time?        
% %                 end                
% % %                 if good_sign &&~good_pos% then system has just started to move in the right direction, but at the wrong velocity, increment adjustment level and switch adjustment direction
% % %                     par_adj_level=par_adj_level/2;
% % %                     par_adj_dir=-par_adj_dir;
% % %                 end
% %                 if (((prev_good_min_val==1) && (good_min_val==0)) || ((prev_good_sign==1) && (good_sign==0))) %if system state has stopped moving towards desired state
% %                     par_adj_level(param_arr_idx_vec_idx)=par_adj_level(param_arr_idx_vec_idx)/2;
% %                     par_adj_dir(param_arr_idx_vec_idx)=-par_adj_dir(param_arr_idx_vec_idx);                    
% %                 end% if ~good_min_val || (prev_good_sign && ~good_sign)
% %                 if ((prev_good_min_val==0) && (good_min_val==0))% if parameter decreases have not caused ystem to start moving into desired state, begin parameter increase. Note that prev_good_min_val must have been initialized (after a sequence of 2 valid acceleration pulses) before this test can pass
% %                     par_adj_level(param_arr_idx_vec_idx)=par_adj_level(param_arr_idx_vec_idx)*2;
% %                     par_adj_dir(param_arr_idx_vec_idx)=-par_adj_dir(param_arr_idx_vec_idx);                                        
% %                 end% if (~prev_good_min_val && ~good_min_val)
% %             end% if (cur_min_val~=prev_min_val)
%             if good_min_val==-1
%                 good_min_val=0;% initialize on first valid pulse
%             end
%             if prev_good_min_val==-1
%                 prev_good_min_val=0;
%             end            
%         else
%             %if there is an invalid acceleration pulse, reset the flags to
%             %-1 to reinitialize pulse sequence detection logic
%         end%  if ((cur_pulse_count==(1+prev_pulse_cnt))||(cur_pulse_count==prev_pulse_cnt))&&(loop_counter>=2)
%         %have we reached perfoamce goal?
%         %goal: 1) increment level== min increment level.  2)previous sign
%         %good, current sign bad.  I.E.: Adjusting the current parameter by
%         %the minimum amount causes the system to begin exhibiting undesired
%         %behavior. Revert back to the previous parameter settings and break
%         %loop
%         if ~goal_reached
%             prev_value=[prev_value round(RG*[param_arr(:);cur_pulse_aaf_sign;clk_pe(end);cur_min_val])/RG];% record the parameter values used during search and wether they produced a good signal
% %[par_adj_level(param_arr_idx_vec_idx) par_adj_dir(param_arr_idx_vec_idx)]
%                 param_arr(param_arr_idx)=round(RG*(param_arr(param_arr_idx)+par_adj_level(param_arr_idx_vec_idx)*par_adj_dir(param_arr_idx_vec_idx)))/RG;% for rounding puposes later, want RG*param value to be an integer                      
%             % if there's a repitition in parameter value then assign
%             % parameter value to be last known good value (row 2==1)
%             %repetition indicates that parameters have been adjusted past
%             %the rounding limit, so search should stop
%             if loop_counter>2
%                 test1=(prev_value(4,end)==prev_value(4,end-1))&&(prev_value(4,end)==(-prev_pulse_aaf_sign));
%                 test2=prev_value(6,end)==prev_value(6,end-1);
%                 check1=(test1==1)&&(test2==1);% array of good sign and inter-trial target differences
% 
%                 test1=((prev_value(4,end)==prev_value(4,end-1))&&(prev_value(4,end)==prev_value(4,end-2)))&&(prev_value(4,end)==(-prev_pulse_aaf_sign));
%                 test2=(prev_value(6,end)==prev_value(6,end-1))&&(prev_value(6,end)==prev_value(6,end-2));
%                 check2=(test1==1)&&(test2==2);% check if 3 contiguous trials produce the same result under *invalid* system states
%                 check3=any(abs(prev_value(param_arr_idx_vec(param_arr_idx_vec_idx),2:size(prev_value,2)))<=1/RG);% reached the smallest allowable value.  Assume some satisfacory state has occured in the past series of trials
%                 if check1||check2||check3
%                    %keep the best column of prev_value
%                    idx=find(prev_value(4,:)==-prev_pulse_aaf_sign);
%                    [min_val min_val_idx]=min(prev_value(6,idx));% of the valid system states, whicj one causes the ystem to linger near equilibrium the longes?
%                    prev_value=prev_value(:,idx(min_val_idx));
%                    param_arr(param_arr_idx)= prev_value(param_arr_idx_vec(param_arr_idx_vec_idx),1);
%                    param_arr_idx_vec_idx=param_arr_idx_vec_idx+1;% move to the next parameter to begin search again
%                    if check2
%                        good_sign=1;% even thoug hthis isn't true, reset this flag to enable proper parameter search
%                    end
%                    if param_arr_idx_vec(param_arr_idx_vec_idx)~=-1% then we have not reached the end of the parameter list to adjust
%                        param_arr_idx=param_arr_idx_vec(param_arr_idx_vec_idx);
%                        param_arr(param_arr_idx)=round(RG*(param_arr(param_arr_idx)+par_adj_level(param_arr_idx_vec_idx)*par_adj_dir(param_arr_idx_vec_idx)))/RG    ;                   
%                    else% we've reached the end of the parameter list
%                        goal_reached=1;
%                        do_adj=0;
%                        %execute simulation with established parameters
%                         TLDAF(APForder,AVForder,AAForder,APHorder)=param_arr(1);
%                         TLDT(APForder,AVForder,AAForder,APHorder)=T;% for now, T is constant     
%                         TLDST1(APForder,AVForder,AAForder,APHorder)=param_arr(3);
%                         [t,x,apf,avf,aaf,aph,apf_pe,avf_pe,aaf_pe,aph_pe,clk_pe,pulse_cnt]=sim('ipend3',sim_time); 
%                         last_AF=param_arr(1);
%                    end
%                 end% check1
%             end% if loop_counter>=2
%         end% if ~goal_reached
%     end% while do_adj

%     idx=min(find(clk_pe==clk_pe(end)));% the vector element index of the end-of-pulse time
%     t_idx_avf0_x=crossing(avf);
%     t_idx_avf0_x=t_idx_avf0_x(min(find(t_idx_avf0_x>=idx)));% find the avf zero crossing time index greater than or equal to the pulse end time        
%     if isempty(t_idx_avf0_x)||any(abs(apf(1:t_idx_avf0_x))>=(pi/2))% if avf never crosses 0 or it crosses 0 after apf crosses pi/2
%         prev_pulse_aaf_sign=sign(avf(idx));% sign of acceleration at time of velocity zero crosssing, of, if there isn;t one, the time of pulse end
%     else
%         prev_pulse_aaf_sign=sign(aaf(t_idx_avf0_x));% direction of foot angular acceleration due to the previous pulse. -1=clockwise
%     end

%% ADD NEW INITAL VALUES TO BREAKPOINT ARRAYS
    [TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,APForder,AVForder,AAForder,APHorder]=InsertBlankRows(TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end));
    DATA=[DATA;apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end),clk_pe(end)];
    MAX_PULSES=MAX_PULSES+1;% allow one additional pulse for next trial    
%%
%NOTE: slope of AAF must point in direction( negative/positive) that system
%is desired to go.  It must have this slope at the end of the input pulse
% so, take start time, add 2*T+.01 for error delay, examine AAF at htis
% time.  Lowering AF will lower the slope at a given time
        TLDAF(APForder,AVForder,AAForder,APHorder)=param_arr(1);%0;0;0;-11.01;
        TLDT(APForder,AVForder,AAForder,APHorder)=T;% f0or now, T is constant     
        TLDST1(APForder,AVForder,AAForder,APHorder)=0;%param_arr(3)+0;1.356%1.403%1.072;2.707;0
        [t,x,apf,avf,aaf,aph,apf_pe,avf_pe,aaf_pe,aph_pe,clk_pe,pulse_cnt]=sim('ipend3',5); 
%%
        last_AF=param_arr(1);
        if aph_pe(end)>0
            target_aph=-0.01;
        else
            target_aph=0.01;
        end
end
%0.246 1.033 1.716 1.446 1.37 .3 .4 .2  
%83.333 -115  35 -77
%60 -62 5 -5 8


%     do_adj=1;
%     MAX_PULSES=MAX_PULSES+1;% allow one additional pulse for next trial
%     delete MEMORY.mat;
%     save 'MEMORY' *BP TLD* IFA MS RG MAX_PULSES;% save learned data and system state
%
